#include "location.hpp"
#include "evtinfo.hpp"
#include "nlohmann/json.hpp"
#include "posinfo.hpp"
#include <arpa/inet.h>
#include <cmath>
#include <cstring>
#include <iostream>
#include <netinet/in.h>
#include <queue>
#include <sys/socket.h>
#include <thread>
#include <unistd.h>

#define UPD_UPLOAD 0

Location::Location()
{
    int ret = 0;

#if 0
    std::string url = "ws://192.168.170.33:30000/gateway/robot-alarm/ws/robot?deviceIndexCode=A000001";
#else
    std::string url = "wss://vlog16.aiit.org.cn:30000/gateway/robot-alarm/ws/robot?deviceIndexCode=A000001";
#endif
    webcli.config(url);
    ret = webcli.connect();
    if (ret) {
        std::cout << "ws websocket connect faild!" << std::endl;
        return;
    }
    std::cout << "wss websocket connect success!" << std::endl;
}

Location::~Location()
{
    webcli.close();
}

/**
 * @brief 上报位置信息
 */
void Location::upload_thread()
{
    bool valid;
    float x = 0.0f;
    float y = 0.0f;
    float a = 0.0f;

#if UPD_UPLOAD
    std::string upd_ip = "192.168.170.41";
    int upd_port = 5050;
    int udp_fd = socket(AF_INET, SOCK_DGRAM, 0);
    struct sockaddr_in outaddr;
    memset(&outaddr, 0, sizeof(outaddr));
    outaddr.sin_family = AF_INET;
    outaddr.sin_port = htons(upd_port);
    inet_pton(AF_INET, upd_ip.data(), &outaddr.sin_addr);
#endif

    while (upload_running) {
        PosInfo::get(valid, x, y, a);
        std::cout << "Location, " << valid << " point : " << x << ", " << y << ", " << a << std::endl;

        if (false == valid) {
            msleep(50);
            continue;
        }

        /* PGM 分辨率, 原点偏移, 图片高度 */

#if 0
        /* 世纪城派出所花园 */
        float resolution = 0.05;
        float origin[2] = { -38.3, -42.6 };
        int height = 1787;
#endif

#if 1
        /* 智慧谷1楼展厅 */
        float resolution = 0.05;
        float origin[2] = { -18.2, -11.1 };
        int height = 312;
#endif

        /* 3D 雷达坐标系转 PGM 图片像素坐标 */
        int pgm_x = int((x - origin[0]) / resolution);
        int pgm_y = height - int((y - origin[1]) / resolution);
        float pgm_a = 0 - a;

#if UPD_UPLOAD
        /* 构造 JSON 字符串 */
        nlohmann::json udp_json;
        udp_json["x"] = pgm_x;
        udp_json["y"] = pgm_y;
        udp_json["a"] = pgm_a;
        std::string udp_message = udp_json.dump();

        /* UDP 发送 JSON 数据 */
        sendto(udp_fd, (const char*)udp_message.data(), udp_message.length(), MSG_CONFIRM, (struct sockaddr*)&outaddr, sizeof(struct sockaddr_in));
#endif

        /* 创建 pos_payload 对象 */
        nlohmann::json pos_payload;
        pos_payload["x"] = pgm_x;
        pos_payload["y"] = pgm_y;

        /* 创建主 pos_json 对象 */
        nlohmann::json pos_json;
        pos_json["eventId"] = std::rand();
        pos_json["event"] = "lidar";
        pos_json["operate"] = "report";
        pos_payload["timestamp"] = 0;
        pos_json["payload"] = pos_payload;
        std::string message = pos_json.dump();

        int ret = webcli.send(message);
        if (ret) {
            std::cout << "wss websocket send faild!" << std::endl;
            msleep(1000);
            continue;
        } else {
            // std::cout << "wss websocket send pos " << message << " success!" << std::endl;
        }

        if (send_flag) {
            send_flag = 0;

            /* 发送事件消息 */
            /* 创建 payload 对象 */
            nlohmann::json move_payload;
            move_payload["indexCode"] = index_code;

            /* 创建主 json 对象 */
            nlohmann::json move_json;
            move_json["eventId"] = event_id;
            move_json["event"] = "move";
            move_json["operate"] = "report";
            move_json["timestamp"] = 0;
            move_json["payload"] = move_payload;
            std::string message = move_json.dump();

            int ret = webcli.send(message);
            if (ret) {
                std::cout << "wss websocket send faild!" << std::endl;
                msleep(1000);
                continue;
            } else {
                std::cout << "wss websocket send move " << message << " success!" << std::endl;
            }
        }

        msleep(500);
    }
}

/**
 * @brief 接收服务器消息
 */
void Location::receive_thread()
{
    int ret = 0;
    std::string buffer;

    while (receive_running) {
        int ret = webcli.receive(buffer);
        if (ret) {
            std::cout << "wss websocket receive faild!" << std::endl;
            msleep(1000);
            continue;
        } else {
            // std::cout << "read: " << buffer << std::endl;
            nlohmann::json json = nlohmann::json::parse(buffer);
            if (json["event"] == "move" && json["operate"] == "push") {
                event_id = json["eventId"].get<std::string>();
                index_code = json["payload"]["indexCode"].get<std::string>();
                std::cout << "event: move, id = " << event_id << ", code = " << index_code << std::endl;
                EvtInfo::set(true, EVT_TYPE_MOVE);
            } else if (json["event"] == "move" && json["operate"] == "recover") {
                std::cout << "event: move recover, id = " << event_id << ", code = " << index_code << std::endl;
                EvtInfo::set(true, EVT_TYPE_RECOVER);
            }
        }
    }
}

void Location::start()
{
    if (!upload_handle.joinable()) {
        upload_running = true;
        upload_handle = std::thread(&Location::upload_thread, this);
    }

    if (!receive_handle.joinable()) {
        receive_running = true;
        receive_handle = std::thread(&Location::receive_thread, this);
    }
}

void Location::stop()
{
    if (upload_handle.joinable()) {
        upload_running = false;
        upload_handle.join();
    }

    if (receive_handle.joinable()) {
        receive_running = false;
        receive_handle.join();
    }
}

void Location::set_send_flag()
{
    send_flag = 1;
}

void Location::msleep(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}
